#include "cv_measure.h"

//*********************** 三维建模 参数 ********************************
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q

Mat xyz;              //三维坐标
Mat canvas_main;
Mat color_main;

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象


Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);

/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0  1
*/
/*//新摄像头
Mat cameraMatrixL = (Mat_<double>(3, 3) << 4.2420371939684514e+002,0.,1.5950000000000000e+002,0.,
                     4.2420371939684514e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffL = (Mat_<double>(5, 1) << -3.7198889560662229e-001,-1.3654923131896908e-002,
                  4.4813893788278160e-003,-2.3706251542376924e-003, 0.);

Mat cameraMatrixR = (Mat_<double>(3, 3) <<  4.2420371939684514e+002,0.,1.5950000000000000e+002,0.,
                     4.2420371939684514e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffR = (Mat_<double>(5, 1) << -3.6255158274727367e-001,-9.3817147538152817e-002,
                  3.7549487412259198e-003, 4.8583611681833670e-004,0.);

Mat T = (Mat_<double>(3, 1) << -6.0374462929120007e+001,3.7453075677379138e-001,
         -1.2406483067906511e-001);//T平移向量
Mat rec = (Mat_<double>(3, 1) <<0.04345, -0.05236, -0.01810);//rec旋转向量
Mat R = (Mat_<double>(3, 3) << 9.9969660806031857e-001,-4.9821327588756951e-003,
         2.4121985529112638e-002,4.7142126751139896e-003,
         9.9992671257443977e-001,1.1151039365350269e-002,
         -2.4175773649411435e-002,-1.1033940059957476e-002,
         9.9964682970297347e-001);//R 旋转矩阵
*/
//旧摄像头

Mat cameraMatrixL = (Mat_<double>(3, 3) << 1.2339860093035769e+002,0.,1.5950000000000000e+002,0.,
                     1.2339860093035769e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffL = (Mat_<double>(5, 1) << -2.0502066802024815e-001,3.3022983017958404e-002,
                  -1.8490109355284588e-003,2.2994834795321515e-004, 0.);

Mat cameraMatrixR = (Mat_<double>(3, 3) <<  1.2339860093035769e+002,0.,1.5950000000000000e+002,0.,
                     1.2339860093035769e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffR = (Mat_<double>(5, 1) << -2.0458306584931799e-001,3.2192694197183291e-002,
                  -1.4535135824444120e-003,-4.5178204233773641e-003,0.);

Mat T = (Mat_<double>(3, 1) << -6.4941163923895417e+001,2.7501802237973463e+000,
         -3.0609471356969959e+000);//T平移向量
Mat rec = (Mat_<double>(3, 1) <<0.04345, -0.05236, -0.01810);//rec旋转向量
Mat R = (Mat_<double>(3, 3) << 9.9814224475762925e-001,-2.1547218229059175e-003,
         -6.0888557250659850e-002,3.7761357525184672e-003,
         9.9964097308411126e-001,2.6526698441940152e-002,
         6.0809538963714743e-002,-2.6707341786800336e-002,
         9.9779202134798783e-001);//R 旋转矩阵


/*另一标定结果,对远景比较友好
Mat cameraMatrixL = (Mat_<double>(3, 3) << 1.3920100224335209e+002,0.,1.5950000000000000e+002,
                     0.,1.3920100224335209e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffL = (Mat_<double>(5, 1) << -2.0511158046402655e-001,2.7106078761620283e-002,
                  -3.2154604513889525e-003,1.3943220635644119e-003,0.);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 1.3920100224335209e+002,0.,1.5950000000000000e+002,
                     0.,1.3920100224335209e+002,1.1950000000000000e+002,
                     0.,0.,1.);
Mat distCoeffR = (Mat_<double>(5, 1) << -2.0443942834229226e-001,2.3847714124014371e-002,
                  -3.0857662907249283e-003,-2.7080573323468265e-003,0.);

Mat T = (Mat_<double>(3, 1) << -6.4861062996768581e+001,9.9800479067452064e-001,
         -3.5978386647739384e+000);//T平移向量
Mat rec = (Mat_<double>(3, 1) <<0.04345, -0.05236, -0.01810);//rec旋转向量
Mat R = (Mat_<double>(3, 3) << 9.9865673836091950e-001,-7.7404171527417354e-003,
-5.1232849507240129e-002,9.0557202910972190e-003,
9.9963403690952179e-001,2.5490903906729562e-002,
5.1016789945461917e-002,-2.5920613308219619e-002,
9.9836136190729374e-001);//R 旋转矩阵*/

void tf_init()
{
    /*  立体校正    */
    //Rodrigues(rec, R); //Rodrigues变换
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
                  0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

    cout << "tf_init finish";
}
void tf_dis()
{
    cvtColor(rgbImageL, grayImageL, COLOR_BGR2GRAY);
    cvtColor(rgbImageR, grayImageR, COLOR_BGR2GRAY);

//    imshow("ImageL Before Rectify", grayImageL);
//    imshow("ImageR Before Rectify", grayImageR);

    /*  经过remap之后，左右相机的图像已经共面并且行对准了 */
    remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//INTER_LINEAR
    remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    /*  把校正结果显示出来*/
    parallel_lines_Mat(320,true);

    /*  立体匹配    */
    namedWindow("disparity", WINDOW_NORMAL);
    //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
    setMouseCallback("disparity", onMouse, 0);//disparity
    stereo_match(0, 0);
}

void parallel_lines_Mat(double size=320, bool show_flag=1)
{
    //显示在同一张图上,canvas是平行线画布
    Mat canvas;
    double sf;
    int w, h;
    sf = size / MAX(imageSize.width, imageSize.height);
    w = cvRound(imageSize.width * sf);
    h = cvRound(imageSize.height * sf);
    canvas.create(h, w * 2, CV_8UC3);   //注意通道

    //左图像画到画布上
    Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分
    resize(rectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小
//    Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域
//               cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
    //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形
    //cout << "Painted ImageL" << endl;

    //右图像画到画布上
    canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分
    resize(rectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
//    Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
//               cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
    //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
    //cout << "Painted ImageR" << endl;

    //画上对应的线条
    for (int i = 0; i < canvas.rows; i += 16)
        line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
    if(show_flag)
        imshow("rectified", canvas);

    canvas_main = canvas;

    //return canvas;
}

void show_dect_point(vector<Point> obj, Point new_obj, bool show_flag=1)
{
    float x = obj[0].x;
    float y = obj[0].y;
    float width = 15;
    float height = 15;
    int xLeftBottom = static_cast<int>((x - width / 2));
    int yLeftBottom = static_cast<int>((y - height / 2));
    int xRightTop = static_cast<int>((x + width / 2));
    int yRightTop = static_cast<int>((y + height / 2));
    Rect object(xLeftBottom, yLeftBottom,
        xRightTop - xLeftBottom,
        yRightTop - yLeftBottom);
//    rectangle(grayImageL, object, Scalar(0, 100, 100), 2, 8);
//    imshow("show dect", grayImageL);
    rectangle(rgbImageL, object, Scalar(100, 100, 100), 2, 8);
    if(show_flag) imshow("show dect2", rgbImageL);

    {
        float x = new_obj.x;
        float y = new_obj.y;
        float width = 15;
        float height = 15;
        int xLeftBottom = static_cast<int>((x - width / 2));
        int yLeftBottom = static_cast<int>((y - height / 2));
        int xRightTop = static_cast<int>((x + width / 2));
        int yRightTop = static_cast<int>((y + height / 2));
        Rect object(xLeftBottom, yLeftBottom,
            xRightTop - xLeftBottom,
            yRightTop - yLeftBottom);
        rectangle(color_main, object, Scalar(100, 100, 10), 2, 8);
        if(show_flag) imshow("show dect3", color_main);
    }
}

/*static void saveXYZ(const char* filename, const Mat& mat)
{
    const double max_z = 16.0e4;
    FILE* fp = fopen(filename, "wt");
    printf("%d %d \n", mat.rows, mat.cols);
    for (int y = 0; y < mat.rows; y++)
    {
        for (int x = 0; x < mat.cols; x++)
        {
            Vec3f point = mat.at<Vec3f>(y, x);
            if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);

        }
    }
    fclose(fp);
}*/

/*给深度图上色*/
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
    // color map
    float max_val = 255.0f;
    float map[8][4] = { { 0,0,0,114 },{ 0,0,1,185 },{ 1,0,0,114 },{ 1,0,1,174 },
    { 0,1,0,114 },{ 0,1,1,185 },{ 1,1,0,114 },{ 1,1,1,0 } };
    float sum = 0;
    for (int i = 0; i<8; i++)
        sum += map[i][3];

    float weights[8]; // relative   weights
    float cumsum[8];  // cumulative weights
    cumsum[0] = 0;
    for (int i = 0; i<7; i++) {
        weights[i] = sum / map[i][3];
        cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
    }

    int height_ = src.rows;
    int width_ = src.cols;
    // for all pixels do
    for (int v = 0; v<height_; v++) {
        for (int u = 0; u<width_; u++) {

            // get normalized value
            float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

            // find bin
            int i;
            for (i = 0; i<7; i++)
                if (val<cumsum[i + 1])
                    break;

            // compute red/green/blue values
            float   w = 1.0 - (val - cumsum[i])*weights[i];
            uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
            uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
            uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
            //rgb内存连续存放
            disp.data[v*width_ * 3 + 3 * u + 0] = b;
            disp.data[v*width_ * 3 + 3 * u + 1] = g;
            disp.data[v*width_ * 3 + 3 * u + 2] = r;
        }
    }
}

/*****立体匹配*****/
void stereo_match(int, void*)
{
    sgbm->setPreFilterCap(63);
    int sgbmWinSize =  5;//根据实际情况自己设定
    int NumDisparities = 5*16;//根据实际情况自己设定
    int UniquenessRatio = 6;//根据实际情况自己设定
    sgbm->setBlockSize(sgbmWinSize);
    int cn = rectifyImageL.channels();

    sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(NumDisparities);
    sgbm->setUniquenessRatio(UniquenessRatio);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(10);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(StereoSGBM::MODE_SGBM);
    Mat disp, dispf, disp8;
    sgbm->compute(rectifyImageL, rectifyImageR, disp);
    //去黑边
    Mat img1p, img2p;
    copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, BORDER_REPLICATE);
    copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, BORDER_REPLICATE);
    dispf = disp.colRange(NumDisparities, img2p.cols- NumDisparities);

    dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
    reprojectImageTo3D(dispf, xyz, Q, true);
    xyz = xyz * 16;     //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。

//    cout << "disp.size" << disp.size << endl;
//    imshow("disp",disp);

    imshow("disparity", disp8);
    Mat color(dispf.size(), CV_8UC3);
    GenerateFalseMap(disp8, color);//转成彩图
    imshow("disparity", color);

    color_main = color;
    //saveXYZ("xyz.xls", xyz);
}

/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
    if (selectObject)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
    }

    switch (event)
    {
    case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
        origin = Point(x, y);
        selection = Rect(x, y, 0, 0);
        selectObject = true;
        cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
        break;
    case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            break;
    }
}

